﻿//
// Created by harrybao on 2019/3/27.
//

#include <iostream>
#include "util_math.h"

float UtilMath::ROSin[721];
float UtilMath::ROCos[721];

void UtilMath::Init()
{
    int x = 0;

    float v = 0;
    float d2r = 0.0174532925f;

    while (x < 721)
    {
        ROSin[x] = sin(d2r * (v));
        ROSin[x] *= 1000000;
        ROSin[x] = round(ROSin[x]) / 1000000;

        ROCos[x] = cos(d2r * (v));
        ROCos[x] *= 1000000;
        ROCos[x] = round(ROCos[x]) / 1000000;
        ++x; v += 0.5;
    }
}

Vector3 UtilMath::RotateAroundPoint(const Vector3 &p, const Vector3 &o, float degree)
{
    //???????????????degree??
    int deg = (int) floor(RORoundToCircle(degree) + 0.5f);
    if (deg < 0) deg += 360;
    float sinA = UtilMath::ROSin[deg << 1];
    float cosA = UtilMath::ROCos[deg << 1];

    return Vector3(
            cosA * (p.x() - o.x()) - sinA * (p.z() - o.z()) + o.x(),
            p.y(),
            sinA * (p.x() - o.x()) + cosA * (p.z() - o.z()) + o.z());
}

bool UtilMath::CheckCircleCollision(const Vector3& self_pos, float self_face, const Vector3& self_scale, const Vector3& from, const Vector3& to)
{
    //???????????

    const auto &r = self_scale.x() / 2;
    auto d1 = Vector3::HorizontaledDistance(from, self_pos);
    auto d2 = Vector3::HorizontaledDistance(to, self_pos);
    if ((d1 - r) * (d2 - r) < 0)
    {
        return true;
    }
    if (d1 < r && d2 < r)
    {
        return false;
    }

    //????��????
    Vector3 from_t = from;
    Vector3 to_t = to;
    from_t.SetY(0);
    to_t.SetY(0);
    if (from_t == to_t)
    {
        return false;
    }
    else
    {
        Vector3 v1 = to_t - from_t;
        Vector3 v2 = self_pos - from_t;
        Vector3 v3 = self_pos - to_t;
        v1.SetY(0);
        v2.SetY(0);
        v3.SetY(0);
        if (Vector3::Dot(v1, v2) < 0 || Vector3::Dot(v1, v3) > 0)
            return false;
        //printf("d=%.12f\n", fabs(Vector3::Cross(v1, v2).y()) / Vector3::Magnitude(v1));
        return (fabs(Vector3::Cross(v1, v2).y()) / Vector3::Magnitude(v1)) <= r;
    }
}

bool UtilMath::IsInCircle(const Vector3& self_pos, float self_face, const Vector3& self_scale, const Vector3& pos)
{
    return 2 * Vector3::HorizontaledDistance(pos, self_pos) <= self_scale.x();
}

#define SMALL_OFFSET 0.005f
bool UtilMath::CheckEllipseCollision(const Vector3 & self_pos, float self_face, const Vector3 & self_scale, const Vector3 & from, const Vector3 & to)
{
    //?????????????

    const auto &x0 = self_pos.x();
    const auto &z0 = self_pos.z();

    Vector3 from_t = from;
    Vector3 to_t = to;

    int deg = (int) floor(RORoundToCircle(self_face) + 0.5f);
    if (deg < 0) deg += 360;
    float sinA = UtilMath::ROSin[deg << 1];
    float cosA = UtilMath::ROCos[deg << 1];

    float rk = 0.0f;
    float rt = 0.0f;
    if (ROIsZeroNumber(to.x() - from.x()))
    {//��???????
        to_t.SetX(to_t.x() + SMALL_OFFSET);
        from_t.SetX(from_t.x() - SMALL_OFFSET);
    }
    if (ROIsZeroNumber(to_t.z() - from_t.z()))
    {//?????x??
        to_t.SetZ(to_t.z() + SMALL_OFFSET);
        from_t.SetZ(from_t.z() - SMALL_OFFSET);
    }

    //???(y=kx+t)????????deg???? -> (y=rk*x+rt)
    auto k = (to_t.z() - from_t.z()) / (to_t.x() - from_t.x());
    auto t = to_t.z() - k * to_t.x();
    if (ROIsZeroNumber(sinA))
    {
        rk = k;
        rt = t;
    }
    else
    {//��?????
        rk = (sinA + k * cosA) / (cosA - k * sinA);
        rt = (x0 * (k - sinA - k * cosA) + z0 * (cosA - 1 - k * sinA) + t) / (cosA - k * sinA);
    }

    auto aa = self_scale.x() * self_scale.x() / 4;
    auto bb = self_scale.z() * self_scale.z() / 4;

    auto A = (1 / aa + rk * rk / bb);
    auto B = 2 * (-x0 / aa + (rt - z0) * rk / bb);
    auto C = x0 * x0 / aa + (rt * rt + (z0 - 2 * rt) * z0) / bb - 1;
    auto delta = B * B - 4 * A * C;


    if (from_t.x() > to_t.x())
    {
        std::swap(from_t, to_t);
    }

    if (delta < 0)
    {//????
        return false;
    }
    else if (ROIsZeroNumber(delta))
    {//????
        auto x1 = -B / A / 2;
        Vector3 v1(x1, 0, x1 * rk + rt);
        if (!ROIsZeroNumber(sinA))
        {
            v1 = RotateAroundPoint(v1, self_pos, -self_face);
        }
        return (from_t.x() <= x1 && x1 <= to_t.x());
    }
    else
    {
        //???????
        auto x1 = (-B - sqrt(delta)) / A / 2;
        auto x2 = (-B + sqrt(delta)) / A / 2;

        Vector3 v1(x1, 0, x1 * rk + rt);
        Vector3 v2(x2, 0, x2 * rk + rt);
        if (!ROIsZeroNumber(sinA))
        {//??????
            v1 = UtilMath::RotateAroundPoint(v1, self_pos, -self_face);
            v2 = UtilMath::RotateAroundPoint(v2, self_pos, -self_face);
        }

        //DEBUG
        //printf("p1(%.12f, %.12f)\np2(%.12f, %.12f)\n", v1.x(),v1.z(),v2.x(),v2.z());

        //?��???????????
        return (from_t.x() <= v1.x() && v1.x() <= to_t.x()) || (from_t.x() <= v2.x() && v2.x() <= to_t.x());
    }
}

#define ENLARGE_FACTOR 100
bool UtilMath::CheckEllipseCollisionOld(const Vector3 & self_pos, float self_face, const Vector3 & self_scale, const Vector3 & from, const Vector3 & to)
{
    //TODO
    //?????????????

    auto x0 = self_pos.x() * ENLARGE_FACTOR;
    auto z0 = self_pos.z() * ENLARGE_FACTOR;
    auto from_t = from * ENLARGE_FACTOR;
    auto to_t = to * ENLARGE_FACTOR;
    auto scale = self_scale * ENLARGE_FACTOR;

    int deg = (int) floor(RORoundToCircle(self_face) + 0.5f);
    if (deg < 0) deg += 360;
    float sinA = UtilMath::ROSin[deg << 1];
    float cosA = UtilMath::ROCos[deg << 1];

    if (ROIsZeroNumber(to_t.x() - from_t.x()))
    {//TODO
        //��???????
        return false;
    }

    auto k = (to_t.z() - from_t.z()) / (to_t.x() - from_t.x());
    auto t = to_t.z() - k * to_t.x();
    auto aa = scale.x() * scale.x() / 4;
    auto bb = scale.z() * scale.z() / 4;

    auto k1 = cosA - k * sinA;
    auto b1 = -x0 * cosA - (t - z0) * sinA;
    auto k2 = sinA + k * cosA;
    auto b2 = -x0 * sinA + (t - z0) * cosA;

    auto A = (bb + k1 * k1 + aa * k2 * k2);
    auto B = 2 * (bb * k1 * b1 + aa * k2 * b2);
    auto C = bb * b1 * b1 + aa * b2 * b2 - aa * bb;

    auto delta = B * B - 4 * A * C;
    if (delta < 0)
    {//????
        return false;
    }
    else if (ROIsZeroNumber(delta))
    {//????
        auto x1 = -B / A / 2;
        return (from_t.x() <= x1 && x1 <= to_t.x());
    }
    else
    {
        //TODO
        //???????
        auto x1 = (-B - sqrt(delta)) / A / 2;
        auto x2 = (-B + sqrt(delta)) / A / 2;

        //DEBUG
        Vector3 v1(x1, 0, x1 * k + t);
        Vector3 v2(x2, 0, x2 * k + t);


        printf("p1(%.8f, %.8f)\np2(%.8f, %.8f)\n",
               v1.x()/ENLARGE_FACTOR,
               v1.z()/ENLARGE_FACTOR,
               v2.x()/ENLARGE_FACTOR,
               v2.z()/ENLARGE_FACTOR);

        //TODO
        //?��???????????
        if (from_t.x() > to_t.x())
        {
            std::swap(from_t, to_t);
        }
        return (from_t.x() <= x1 && x1 <= to_t.x()) || (from_t.x() <= x2 && x2 <= to_t.x());
    }
}

bool UtilMath::IsInEllipse(const Vector3& self_pos, float self_face, const Vector3& self_scale, const Vector3& pos)
{
    const auto &x0 = self_pos.x();
    const auto &z0 = self_pos.z();
    const auto &a = self_scale.x();
    const auto &b = self_scale.z();

    int32_t deg = static_cast<int32_t>(floor(RORoundToCircle(self_face) + 0.5f));
    if (deg < 0) deg += 360;
    const auto &sinA = UtilMath::ROSin[deg << 1];
    const auto &cosA = UtilMath::ROCos[deg << 1];

    //???????
    auto tx = pos.x() - x0;
    auto tz = pos.z() - z0;
    auto dx = tx * cosA - tz * sinA;  // +x0 - x0
    auto dz = tx * sinA + tz * cosA;  // +z0 - z0

    auto d = (dx * dx / a / a + dz * dz / b / b) * 4;
    return (d - 1) <= EPSINON;
}

bool UtilMath::IsInRect(const Vector3& point, const Vector3* pvecs, const Vector3& center, float yRot)
{
    const Vector3& v = Vector3::HorizontalRotate(point - center, -yRot, false);
    // std::cout << "------" << "\n";
    // std::cout << point.ToString() << "\n";
    // std::cout << center.ToString() << "\n";
    // std::cout << (point - center).ToString() << "\n";
    // std::cout << v.ToString() << "\n";
    return (v .x() > pvecs[0].x() && v.x() < pvecs[3].x() && v.z() > pvecs[0].z() && v.z() < pvecs[2].z());
}

Box3D * Box3D::CreateBox3D(const Vector3& pos, const Vector3& scale, const float& face_degree)
{
    Box3D* ret = new Box3D();
    ret->wall_min_[0] = - scale.x();
    ret->wall_min_[1] = - scale.y();
    ret->wall_min_[2] = - scale.z();
    ret->wall_max_[0] = scale.x();
    ret->wall_max_[1] = scale.y();
    ret->wall_max_[2] = scale.z();
    ret->face_degree_ = face_degree;
    ret->center_pos_ = pos;
    return ret;
}

#define RCEPS 1e-6f
bool Box3D::IsRayCross2D(const Vector3 & start_pos, const Vector3 & end_pos, const Box3D* box)
{
    if (box == nullptr)
    {
        return false;
    }
    Vector3 start_point = start_pos - box->center_pos_;
    start_point.HorizontalRotate(-box->face_degree_);
    Vector3 end_point = end_pos - box->center_pos_;
    end_point.HorizontalRotate(-box->face_degree_);

    float e[3];
    start_point.VectorToFloat(e);
    float s[3];
    end_point.VectorToFloat(s);

    float d[3];
    d[0] = e[0] - s[0];
    d[1] = e[1] - s[1];
    d[2] = e[2] - s[2];
    float tmin = 0.0f;
    float tmax = 1.0f;

    for (int i = 0; i < 3; i++)
    {
        if (i != 1)
        {
            if (fabsf(d[i]) < RCEPS)
            {
                if (s[i] < box->wall_min_[i] || s[i] > box->wall_max_[i])
                {
                    return false;
                }
            }
            else
            {
                const float od = 1.0f / d[i];
                float tmp = 0.0f;
                float t1 = (box->wall_min_[i] - s[i]) * od;
                float t2 = (box->wall_max_[i] - s[i]) * od;
                if (t1 > t2) { tmp = t1; t1 = t2; t2 = tmp; }
                if (t1 > tmin) { tmin = t1; }
                if (t2 < tmax) { tmax = t2; }
                if (tmin > tmax)
                {
                    return false;
                }
            }
        }
    }
    return true;
}

bool Box3D::IsRayCross3D(const Vector3 & start_pos, const Vector3 & end_pos, const Box3D* box)
{
    if (box == nullptr)
    {
        return false;
    }
    Vector3 start_point = start_pos;
    start_point.HorizontalRotate(-box->face_degree_);
    Vector3 end_point = end_pos;
    end_point.HorizontalRotate(-box->face_degree_);

    float e[3];
    start_point.VectorToFloat(e);
    float s[3];
    end_point.VectorToFloat(s);

    float d[3];
    d[0] = e[0] - s[0];
    d[1] = e[1] - s[1];
    d[2] = e[2] - s[2];
    float tmin = 0.0f;
    float tmax = 1.0f;

    for (int i = 0; i < 3; i++)
    {
        if (fabsf(d[i]) < RCEPS)
        {
            if (s[i] < box->wall_min_[i] || s[i] > box->wall_max_[i])
            {
                return false;
            }
        }
        else
        {
            const float od = 1.0f / d[i];
            float tmp = 0.0f;
            float t1 = (box->wall_min_[i] - s[i]) * od;
            float t2 = (box->wall_max_[i] - s[i]) * od;
            if (t1 > t2) { tmp = t1; t1 = t2; t2 = tmp; }
            if (t1 > tmin) { tmin = t1; }
            if (t2 < tmax) { tmax = t2; }
            if (tmin > tmax)
            {
                return false;
            }
        }
    }
    return true;
}

extern "C" int add(int a,int b)
{
    return (a + b);
}

extern "C" int sub(int a,int b)
{
    return (a - b);
}

extern "C" int mul(int a,int b)
{
    return (a * b);
}

extern "C" int dvd(int a,int b)
{
    return (a / b);
}